#include <stdio.h>
#include <libplayerc/playerc.h>
#include <time.h>
#include <unistd.h>



#define MAX_TEAM 5

typedef enum t_estado_robo{
    ESTADO_PROCURANDOBANDEIRA=0,
    ESTADO_ANDANDOALEATORIO,
    ESTADO_CAPTURANDOBANDEIRA,
    ESTADO_VOLTANDO,
    ESTADO_PERSEGUINDO,
    ESTADO_FINAL,
    ESTADO_PROCURANDOINIMIGO
} EstadoRobo;

typedef struct t_color{
    unsigned int R,G,B;
} Color;

typedef struct t_robo{
    playerc_client_t *client;
    playerc_position2d_t *position2d;
    playerc_laser_t *laser;
    playerc_blobfinder_t *bf;
    playerc_gripper_t *gripper;
    float vel_linear;
    float vel_angular;
    int porta;
    int desviandoObstaculo;
    float initial_x;
    float initial_y;
    EstadoRobo state;
    unsigned short int has_the_flag;
    Color bandeira;
    char robotTeam;
} Robo;



typedef struct t_time{
    Robo member[MAX_TEAM];
    EstadoRobo state;
    int bandeiraX,bandeiraY;    //indica a posicao da bandeira a ser caputurada
    int inimigoX,inimigoY; //indica a posicao do inimigo qndo este caputurar a bandeira do time

}TimeRobos;

typedef struct t_param_desvio{
    float vel_linear_wo_obstacle;
    float vel_linear_w_obstacle;
    float vel_ang_wo_obstacle;
    float vel_ang_w_obstacle;
    float dist_to_obstacle;
}DriveParam;

static const DriveParam SAFE_DRIVE = {0.5,0.1,0,0.7,1};
static const DriveParam NORMAL_DRIVE = {0.6,0.1,0,0.7,0.7};
static const DriveParam SPORT_DRIVE = {0.7,0.2,0,0.7,0.7};
static const DriveParam INSANE_DRIVE = {0.8,0.3,0,0.5,0.8};


static const Color yellow = {255,255,0};
static const Color red = {255,0,0};
static const Color blue = {0,0,255};
static const Color green = {0,255,0};

short redFlagTaken=0;
short blueFlagTaken=0;


int InicializaRobo(Robo *r,char* endereco,int porta,Color bandeiracor,char robotTeam){
	r->client = playerc_client_create(NULL, endereco, porta);
  	if (playerc_client_connect(r->client) != 0)
    		return -1;

  	r->position2d = playerc_position2d_create(r->client, 0);
  	if (playerc_position2d_subscribe(r->position2d, PLAYERC_OPEN_MODE) != 0){
    		fprintf(stderr, "error: %s\n", playerc_error_str());
    		return -1;
  	}

  	r->laser = playerc_laser_create(r->client, 0);
  	if (playerc_laser_subscribe(r->laser, PLAYERC_OPEN_MODE))
    		return -1;

  	r->bf = playerc_blobfinder_create(r->client, 0);
  	if (playerc_blobfinder_subscribe(r->bf, PLAYER_OPEN_MODE))
    		return -1;

  	r->gripper = playerc_gripper_create(r->client, 0);
  	if (playerc_gripper_subscribe(r->gripper, PLAYER_OPEN_MODE))
    		return -1;

	playerc_position2d_enable(r->position2d, 1);
        
        playerc_gripper_set_cmd(r->gripper, GRIPopen, 0);
        r->initial_x=r->position2d->px;
        r->initial_y=r->position2d->py;
        r->state=ESTADO_PROCURANDOBANDEIRA;
        r->has_the_flag=0;
        r->bandeira=bandeiracor;
        r->robotTeam=robotTeam;

return 0;

}
//muda o estado do time todo, exceto do robo que chamou
void changeTeamState(Robo time[],EstadoRobo toSTATE){
    int j;
    for (j=0;j<MAX_TEAM;j++){
        if(!time[j].has_the_flag)
            time[j].state=toSTATE;
    }
}

void gira90(Robo *r){
float lin;
puts("girando 90 graus");
lin=r->vel_linear;
r->vel_linear=-0.3;
r->vel_angular=-0.7;
playerc_position2d_set_cmd_vel(r->position2d, r->vel_linear, 0, r->vel_angular, 1);
usleep(170000);
r->vel_linear=lin;
r->vel_angular=0;
playerc_position2d_set_cmd_vel(r->position2d, r->vel_linear, 0, r->vel_angular, 1);
puts("fim do giro");
}

void desviaObstaculos(Robo* r,DriveParam d)
{
    float distance[5];
    playerc_client_read(r->client);
    int i;
    float smallestDistToObjAhead;

    for (i = 0; i < 3; i++)
    {
      distance[i] = r->laser->scan[90*i][0];
    }
    smallestDistToObjAhead = distance[2];

    for (i = 0; i < 90; i++)
    {
      if (smallestDistToObjAhead > r->laser->scan[90 + 2*i][0])
      {
	smallestDistToObjAhead = r->laser->scan[90 + 2*i][0];
      }
    }
     //printf("smallestDist: %f\n",smallestDistToObjAhead);

    // se a nao ha objetos na frente, anda para frente
    if (smallestDistToObjAhead > d.dist_to_obstacle)
    {
      r->desviandoObstaculo = false;
      r->vel_linear = d.vel_linear_wo_obstacle;

      r->vel_angular = 	d.vel_ang_wo_obstacle;
    }
    else
    {
      // se ha objetos na frente, para e desvia deles

      if (!r->desviandoObstaculo)
      {
         r->vel_linear = d.vel_linear_w_obstacle;
         if (distance[0] - distance[4] > 0)
           r->vel_angular = -d.vel_ang_w_obstacle;
         else if (distance[0] - distance[4] < 0)
           r->vel_angular = d.vel_ang_w_obstacle;
         if (distance[1] - distance[3] > 0)
           r->vel_angular = -d.vel_ang_w_obstacle;
         else if (distance[1] - distance[3] < 0)
       	   r->vel_angular = d.vel_ang_w_obstacle;
      }
      r->desviandoObstaculo = true;
    }
      playerc_position2d_set_cmd_vel(r->position2d, r->vel_linear, 0, r->vel_angular, 1);

}


void goToXY(Robo* r,float x,float y)//robo vai para determinada posicao do mapa, desviando de obstaculos
{
    
    playerc_client_read(r->client);
  //  int i;
    float ang_dest,ang_atual,dist_dest;

    desviaObstaculos(r,SPORT_DRIVE);
    if(!r->desviandoObstaculo){
        ang_dest=atan2(y - r->position2d->py,x - r->position2d->px);
      //  printf("X: %.2f,Y:%.2f\n\n",x,y);
       // ang_dest=atan2(y,x);
        ang_atual=r->position2d->pa;
        dist_dest=sqrt((y - r->position2d->py)*(y - r->position2d->py)+(x - r->position2d->px)*(x - r->position2d->px));
        printf("Ang_dest: %.1f\t Ang_atual: %.2f\tDist: %.1f\n",ang_dest,ang_atual,dist_dest);
               //funcao modulo
              //  if(ang_dest<0) ang_dest=-1*ang_dest;
              //  if(ang_atual<0) ang_atual=-1*ang_atual;
                

        r->vel_linear=0;
        r->vel_angular=0;
        //ang_ready=0;

        int aux=0;
        if (ang_atual > ang_dest+0.15){
            if(ang_atual+ang_dest<0.2 && aux > 7)//necessario pois o angulo as vezes muda de sinal bruscamente
                r->vel_angular=0.8;
            else
                r->vel_angular=-0.8;
            puts("angulo maior")  ;
            aux--;
        }
        else{
        if(ang_atual < ang_dest-0.15){
            if(ang_atual+ang_dest<0.2 && aux > 7)
                r->vel_angular=-0.8;
            else
                r->vel_angular=0.8;
            puts("angulo menor");
            aux--;
        }
        else{
            puts("angulo correto");
            aux++;
       // if((ang_atual < ang_dest +0.1) && (ang_atual > ang_dest - 0.1)){
                   r->vel_linear=0.7;
                   r->vel_angular=0;
            if(dist_dest < 0.2){
                r->vel_linear=0;

                playerc_gripper_set_cmd(r->gripper, GRIPopen, 0);
                usleep(2*10e5);
                printf("OBJETIVO ALCANCADO!\n");
                r->state=ESTADO_FINAL;
                return;
            }
        }
        }

        playerc_position2d_set_cmd_vel(r->position2d, r->vel_linear, 0, r->vel_angular, 1);
    }

}


void findEnemy(Robo *robo,Color e_cor){//cor do inimigo 
    int red,green,blue,k;

    desviaObstaculos(robo,SAFE_DRIVE);
//playerc_position2d_set_cmd_vel(robo->position2d, robo->vel_linear, 0, robo->vel_angular, 1);
    for(k = 0; k < robo->bf->blobs_count; k++){    //atualiza o blob e procura por uma cor
        red=robo->bf->blobs[k].color >> 16;
        green=(robo->bf->blobs[k].color & 0x00ff00) >> 8;
        blue=robo->bf->blobs[k].color & 0x0000ff;
       // printf("R %d, G %d, B %d\n",red,green,blue);

        if(red==e_cor.R && blue==e_cor.B && green==e_cor.G){//achou o inimigo
            printf("ACHOU INIMIGO!!MUDANDO PARA ESTADO PERSEGUINDO...\n");
          //  printf("Blob[%u] area:%4u, x:%2u, y:%2u\n", k, robo->bf->blobs[k].area, robo->bf->blobs[k].x, robo->bf->blobs[k].y);
            robo->state=ESTADO_PERSEGUINDO;

        }
    }
}

void destroyEnemy(Robo *r){
    int k=0,red,green,blue,index_flag=0;
    static int achou=0;
    double dist_alvo[3],dist;


    playerc_client_read(r->client);
    r->vel_linear=0.5;//aumenta a velocidade para pegar logo a bandeira;
    dist_alvo[0]=r->laser->scan[175][0];
    dist_alvo[1]=r->laser->scan[180][0];
    dist_alvo[2]=r->laser->scan[185][0];

    dist=dist_alvo[1];
    for(k=0;k<3;k++){
        if(dist_alvo[k]<dist)
            dist=dist_alvo[k];
    }

    printf("MenorDistanciaInimigo: %.2f\n",dist);

    if(dist > 0.2 && dist < 5){//se esta se aproximando procura pela bandeira
        for(k = 0; k < r->bf->blobs_count; k++){    //atualiza o blob e procura por uma cor
            red=r->bf->blobs[k].color >> 16;
            green=(r->bf->blobs[k].color & 0x00ff00) >> 8;
            blue=r->bf->blobs[k].color & 0x0000ff;
       // printf("R %d, G %d, B %d\n",red,green,blue);

            if(red==r->bandeira.R && blue==r->bandeira.B && green==r->bandeira.G){//achou a bandeira
                printf("INIMIGO COM A BANDEIRA!!\n");
                achou=1;
                index_flag=k;
            }

            if((dist<4)&&(!achou)){
                r->state=ESTADO_PROCURANDOINIMIGO;
                puts("Nao esta com a bandeira. Voltando a procurar");
                achou=0;
                gira90(r);
               }       
        }

    }
    if((dist < 1)&& achou) {//se chegou perto colide violentamente contra o inimigo

        r->vel_linear=1;
        usleep(2*10e5);
        puts("CRASH!!");
        r->state=ESTADO_PROCURANDOBANDEIRA;

    }
    if(r->bf->blobs[index_flag].x > 40)
        r->vel_angular=-0.3;

    else {
        if(r->bf->blobs[index_flag].x < 40)
            r->vel_angular=0.3;
        else
            r->vel_angular=0;
    }

    playerc_position2d_set_cmd_vel(r->position2d, r->vel_linear, 0, r->vel_angular, 1);

  }




void AtualizaRoboProcurando(Robo *robo,Color cor){
    int red,green,blue,k;

    desviaObstaculos(robo,SAFE_DRIVE);
//playerc_position2d_set_cmd_vel(robo->position2d, robo->vel_linear, 0, robo->vel_angular, 1);
    for(k = 0; k < robo->bf->blobs_count; k++){    //atualiza o blob e procura por uma cor
        red=robo->bf->blobs[k].color >> 16;
        green=(robo->bf->blobs[k].color & 0x00ff00) >> 8;
        blue=robo->bf->blobs[k].color & 0x0000ff;
       // printf("R %d, G %d, B %d\n",red,green,blue);
        
        if(red==cor.R && blue==cor.B && green==cor.G){//achou uma bandeira da cor especificada
                printf("BANDEIRA ENCONTRADA!!MUDANDO PARA ESTADO CAPTURANDO BANDEIRA...\n");
 
                    
                robo->has_the_flag=1;
            // printf("Blob[%u] area:%4u, x:%2u, y:%2u\n", k, robo->bf->blobs[k].area, robo->bf->blobs[k].x, robo->bf->blobs[k].y);
                robo->state=ESTADO_CAPTURANDOBANDEIRA;                
                
                break;
          //  }

        }

    }
  
}

void goForTheFlag(Robo *r){
    int k=0,agarrou=0;
    double dist_alvo;


    playerc_client_read(r->client);
    r->vel_linear=0.7;//aumenta a velocidade para pegar logo a bandeira;
    dist_alvo=r->laser->scan[180][0];
 

    if(dist_alvo > 0.2 && dist_alvo < 0.5 )//se esta se aproximando reduz a velocidade
        r->vel_linear=0.1;
    if(dist_alvo < 0.2) {//se chegou perto, para e agarra
        printf("agarrando bandeira...\n");
        agarrou=1;
      
        r->vel_linear=0;
        r->vel_angular=0;
       if(r->robotTeam=='R')
            blueFlagTaken=1;
        else
            redFlagTaken=1;
    }

    if(r->bf->blobs[k].x > 40)
        r->vel_angular=-0.3;

    else {
        if(r->bf->blobs[k].x < 40)
            r->vel_angular=0.3;
        else
            r->vel_angular=0;
    }
    if(agarrou) r->vel_angular=0;
    playerc_position2d_set_cmd_vel(r->position2d, r->vel_linear, 0, r->vel_angular, 1);
    if (agarrou){//se agarrou o objeto, deve voltar a posicao inicial
        printf("Bandeira Capturada!\n\n");
        //obs: deveria soar algum alarme para o time rival indicando q a bandeira deles foi roubada    
        playerc_gripper_set_cmd(r->gripper, GRIPclose, 0);
        usleep(2*10e5);
        agarrou=0;
        r->state=ESTADO_VOLTANDO;
    }
    
    
  }
void stateManager(Robo *robo,Robo time[]){

    if(redFlagTaken){
        if(robo->robotTeam=='R')
            robo->state=ESTADO_PROCURANDOINIMIGO;
    }
    if(blueFlagTaken){
        if(robo->robotTeam=='B')
            robo->state=ESTADO_PROCURANDOINIMIGO;
    }

switch(robo->state){

            case ESTADO_PROCURANDOBANDEIRA:
                                AtualizaRoboProcurando(robo,robo->bandeira);//procura pela cor da bandeira
                                if(robo->state!=ESTADO_PROCURANDOBANDEIRA){//se achou a bandeira
                                    changeTeamState(time,ESTADO_ANDANDOALEATORIO);
                                    }
                               break;
            case ESTADO_ANDANDOALEATORIO:
                                desviaObstaculos(robo,SAFE_DRIVE);//anda aleatoriamente desviando de obstaculos
                                break;

            case ESTADO_CAPTURANDOBANDEIRA:
                                goForTheFlag(robo);//inicializa o algoritmo para captura da bandeira.
                                break;

            case ESTADO_VOLTANDO:
                                 goToXY(robo,robo->initial_x,robo->initial_y);//volta para a posicao inicial
                                 break;

            case ESTADO_PROCURANDOINIMIGO:
                                if(robo->robotTeam=='R')
                                    findEnemy(robo,blue);//procura pela cor azul
                                else
                                    findEnemy(robo,red);//procura pelos robos vermelhos
                                break;

            case ESTADO_PERSEGUINDO:
                             destroyEnemy(robo);
                             break;

         case ESTADO_FINAL:
                             return;
                             break;

     }
}


void FinalizaRobo(Robo *r){
  playerc_gripper_unsubscribe(r->gripper);
  playerc_gripper_destroy(r->gripper);
  playerc_position2d_unsubscribe(r->position2d);
  playerc_position2d_destroy(r->position2d);
  playerc_client_disconnect(r->client);
  playerc_client_destroy(r->client);

}
/*
void FollowColor(int color,Robo *r,){
    	ang_dest=atan2(y_dest - position2d->py,x_dest - position2d->px);
	ang_atual=position2d->pa;
	if(ang_atual > ang_dest)
		va=-0.5;
	else if(ang_atual < ang_dest)
		va=0.5;
		else
			va=0;
		dist_dest=sqrt((y_dest - position2d->py)*(y_dest - position2d->py)+(x_dest - position2d->px)*(x_dest - position2d->px));

		if(dist_dest > 2.0)
			vel=0.5;
		if(dist_dest < 2.0)
			vel=0.2;
		if(dist_dest < 0.5)
			vel=0;
		playerc_position2d_set_cmd_vel(position2d,vel,0,va,1);
	}
*/


